#include "MeasurE616AlgBackC1ToC5Circle.h"

MeasurE616AlgBackC1toC5Circle::MeasurE616AlgBackC1toC5Circle(int i) : MeasurE616AlgBase(i)
{

}

void MeasurE616AlgBackC1toC5Circle::operator()(const cv::Mat &srcImgTemp, int id, std::map<string, float> &measRes,
                                               int fixtureId)
{
    qDebug() << "E616 alg back c1 to c5 circle start";
    if(!srcImgTemp.data) {
        qDebug() << "E616 empty input image";
        Q_ASSERT(srcImgTemp.data);
        return;
    }

    MeasurE616EdgeDetect obj;
    cv::Mat srcImg = srcImgTemp.clone();
    std::vector<cv::Point2f> resptVec;
    std::vector<float>  rdVec;
    for(auto i=17;i<22;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-17,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point> contours;

        //obj.NewCircleEdge(imgt,tparas,contours);
        std::vector<float> cirres;
        obj.NewSmallCircle(imgt,tparas,cirres);
        cv::Point pt(static_cast<int>(cirres[0]),static_cast<int>(cirres[1]));
        rdVec.push_back(cirres[2]);
        contours.push_back(pt);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            resptVec.push_back(ptsOut);
        }
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI95-C1-diam_pixel"), rdVec[0]*2));
    float fai95c1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI95-C1-diam")], rdVec[0]*2);
    measRes.insert(std::make_pair(string("FAI95-C1-diam_real"), fai95c1));

    measRes.insert(std::make_pair(string("FAI22-C1-X-distance_pixel"), -resptVec[0].x));
    float fai22c1x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C1-X-distance")], -resptVec[0].x);
    measRes.insert(std::make_pair(string("FAI22-C1-X-distance_real"), fai22c1x));

    measRes.insert(std::make_pair(string("FAI22-C1-Y-distance_pixel"), -resptVec[0].y));
    float fai22c1y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C1-Y-distance")], -resptVec[0].y);
    measRes.insert(std::make_pair(string("FAI22-C1-Y-distance_real"), fai22c1y));

    float fai22PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C1-X-distance")],
            m_standardValMap["FAI22-C1-X-distance"]);
    float fai22PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C1-Y-distance")],
            m_standardValMap["FAI22-C1-Y-distance"]);
    double fai22PosPixel = obj.GetPosition(cv::Point2f(-resptVec[0].x, -resptVec[0].y), cv::Point2f(fai22PosPixelX, fai22PosPixelY));
    measRes.insert(std::make_pair(string("FAI22-C1-position_pixel"), fai22PosPixel));
    float fai22PosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C1-position")], fai22PosPixel);
    measRes.insert(std::make_pair(string("FAI22-C1-position_real"), fai22PosDis));

    measRes.insert(std::make_pair(string("FAI95-C2-diam_pixel"), rdVec[1]*2));
    float fai95c2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI95-C2-diam")], rdVec[1]*2);
    measRes.insert(std::make_pair(string("FAI95-C2-diam_real"), fai95c2));

    measRes.insert(std::make_pair(string("FAI22-C2-X-distance_pixel"), -resptVec[1].x));
    float fai22c2x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C2-X-distance")], -resptVec[1].x);
    measRes.insert(std::make_pair(string("FAI22-C2-X-distance_real"), fai22c2x));

    measRes.insert(std::make_pair(string("FAI22-C2-Y-distance_pixel"), -resptVec[1].y));
    float fai22c2y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C2-Y-distance")], -resptVec[1].y);
    measRes.insert(std::make_pair(string("FAI22-C2-Y-distance_real"), fai22c2y));

    float fai22C2PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C2-X-distance")],
            m_standardValMap["FAI22-C2-X-distance"]);
    float fai22C2PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C2-Y-distance")],
            m_standardValMap["FAI22-C2-Y-distance"]);

    double fai22C2PosPixel = obj.GetPosition(cv::Point2f(-resptVec[1].x, -resptVec[1].y), cv::Point2f(fai22C2PosPixelX, fai22C2PosPixelY));
    measRes.insert(std::make_pair(string("FAI22-C2-position_pixel"), fai22C2PosPixel));
    float fai22C2PosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C2-position")], fai22C2PosPixel);
    measRes.insert(std::make_pair(string("FAI22-C2-position_real"), fai22C2PosDis));

    measRes.insert(std::make_pair(string("FAI95-C3-diam_pixel"), rdVec[2]*2));
    float fai95c3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI95-C3-diam")], rdVec[2]*2);
    measRes.insert(std::make_pair(string("FAI95-C3-diam_real"), fai95c3));

    measRes.insert(std::make_pair(string("FAI22-C3-X-distance_pixel"), -resptVec[2].x));
    float fai22c3x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C3-X-distance")], -resptVec[2].x);
    measRes.insert(std::make_pair(string("FAI22-C3-X-distance_real"), fai22c3x));

    measRes.insert(std::make_pair(string("FAI22-C3-Y-distance_pixel"), -resptVec[2].y));
    float fai22c3y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C3-Y-distance")], -resptVec[2].y);
    measRes.insert(std::make_pair(string("FAI22-C3-Y-distance_real"), fai22c3y));

    float fai22C3PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C3-X-distance")],
            m_standardValMap["FAI22-C3-X-distance"]);
    float fai22C3PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C3-Y-distance")],
            m_standardValMap["FAI22-C3-Y-distance"]);

    double fai22C3PosPixel = obj.GetPosition(cv::Point2f(-resptVec[2].x, -resptVec[2].y), cv::Point2f(fai22C3PosPixelX, fai22C3PosPixelY));
    measRes.insert(std::make_pair(string("FAI22-C3-position_pixel"), fai22C3PosPixel));
    float fai22C3PosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C3-position")], fai22C3PosPixel);
    measRes.insert(std::make_pair(string("FAI22-C3-position_real"), fai22C3PosDis));

    measRes.insert(std::make_pair(string("FAI95-C4-diam_pixel"), rdVec[3]*2));
    float fai95c4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI95-C4-diam")], rdVec[3]*2);
    measRes.insert(std::make_pair(string("FAI95-C4-diam_real"), fai95c4));

    measRes.insert(std::make_pair(string("FAI22-C4-X-distance_pixel"), -resptVec[3].x));
    float fai22c4x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C4-X-distance")], -resptVec[3].x);
    measRes.insert(std::make_pair(string("FAI22-C4-X-distance_real"), fai22c4x));

    measRes.insert(std::make_pair(string("FAI22-C4-Y-distance_pixel"), -resptVec[3].y));
    float fai22c4y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C4-Y-distance")], -resptVec[3].y);
    measRes.insert(std::make_pair(string("FAI22-C4-Y-distance_real"), fai22c4y));

    float fai22C4PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C4-X-distance")],
            m_standardValMap["FAI22-C4-X-distance"]);
    float fai22C4PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C4-Y-distance")],
            m_standardValMap["FAI22-C4-Y-distance"]);

    double fai22C4PosPixel = obj.GetPosition(cv::Point2f(-resptVec[3].x, -resptVec[3].y), cv::Point2f(fai22C4PosPixelX, fai22C4PosPixelY));
    measRes.insert(std::make_pair(string("FAI22-C4-position_pixel"), fai22C4PosPixel));
    float fai22C4PosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C4-position")], fai22C4PosPixel);
    measRes.insert(std::make_pair(string("FAI22-C4-position_real"), fai22C4PosDis));

    measRes.insert(std::make_pair(string("FAI95-C5-diam_pixel"), rdVec[4]*2));
    float fai95c5 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI95-C5-diam")], rdVec[4]*2);
    measRes.insert(std::make_pair(string("FAI95-C5-diam_real"), fai95c5));

    measRes.insert(std::make_pair(string("FAI22-C5-X-distance_pixel"), -resptVec[4].x));
    float fai22c5x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C5-X-distance")], -resptVec[4].x);
    measRes.insert(std::make_pair(string("FAI22-C5-X-distance_real"), fai22c5x));

    measRes.insert(std::make_pair(string("FAI22-C5-Y-distance_pixel"), -resptVec[4].y));
    float fai22c5y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C5-Y-distance")], -resptVec[4].y);
    measRes.insert(std::make_pair(string("FAI22-C5-Y-distance_real"), fai22c5y));

    float fai22C5PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C5-X-distance")],
            m_standardValMap["FAI22-C5-X-distance"]);
    float fai22C5PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI22-C5-Y-distance")],
            m_standardValMap["FAI22-C5-Y-distance"]);

    double fai22C5PosPixel = obj.GetPosition(cv::Point2f(-resptVec[4].x, -resptVec[4].y), cv::Point2f(fai22C5PosPixelX, fai22C5PosPixelY));
    measRes.insert(std::make_pair(string("FAI22-C5-position_pixel"), fai22C5PosPixel));
    float fai22C5PosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-C5-position")], fai22C5PosPixel);
    measRes.insert(std::make_pair(string("FAI22-C5-position_real"), fai22C5PosDis));

    qDebug() << "E616 alg back c1 to c5 circle end";
}
